#ifndef PHYSICSENGINE_H
#define PHYSICSENGINE_H

#define MAX_RIGID_BODIES 30
#define PHYS_EPSILON 0
#define MAX_ITERATIONS 1000
#define ITER_SIZE 0.005
#define FRICTION_THRES 0.0001
#define BISECTION_LIMIT 0.005
#define FRICTION_CONSTANT 8

#include "includes.h"
#include "QuickArray.h"
#include "GameStates.h"
#include "RigidBody.h"
#include "Collision.h"
#include "rendermanager.h"

class PhysicsEngine
{
    QuickArray<RigidBody*,MAX_RIGID_BODIES> bodies;

    RenderManager* rend;
    void calcFriction(RigidBody& b);
    void resolveCollisionWithSurface(RigidBody& b);
    void resolveCollisionWithBodies(RigidBody& b);
    vector3<float> refVelocity(vector3<float> velo, vector3<float> norm){
        vector3<float> Vn = norm*(norm.dot(velo));
        //Vt=V-Vn
        return (velo-Vn) - (Vn*GS->getRestitution());
    }
public:
    PhysicsEngine();
    bool checkForVoxel(RigidBody& b, vector3<float>& v){
        return rend->isVoxelDrawn((int)v.x,(int)v.y,(int)v.z) && b.geometry.hasContactedVoxel(v);
    }
    void update();
    void addRigidBody(RigidBody* b){bodies.push(b);}
    void clearAllBodies(){bodies.reset();}
};

#endif // PHYSICSENGINE_H
